//
// Created by lqk on 2022/1/30.
//

#include "DefaultSwingLegPlanner.h"

Eigen::Vector3f CubicBezier::CubicBezierZeroOrder(Eigen::Vector3f p0, Eigen::Vector3f pf, float x) {
    assert(x>=0&&x<=1);
    Eigen::Vector3f pDiff=pf-p0;
    float bezier=x * x * x + float(3) * (x * x * (float(1) - x));
    return p0+bezier*pDiff;
}

Eigen::Vector3f CubicBezier::CubicBezierFirstOrder(Eigen::Vector3f p0, Eigen::Vector3f pf, float x) {
    assert(x >= 0 && x <= 1);
    Eigen::Vector3f pDiff = pf - p0;
    float bezier = float(6) * x * (float(1) - x);
    return bezier * pDiff;
}

Eigen::Vector3f CubicBezier::CubicBezierSecondOrder(Eigen::Vector3f p0, Eigen::Vector3f pf, float x) {
    assert(x >= 0 && x <= 1);
    Eigen::Vector3f pDiff = pf - p0;
    float bezier = float(6) - float(12) * x;
    return bezier * pDiff;
}

float CubicBezier::CubicBezierZeroOrder(float p0, float pf, float x) {
    assert(x>=0&&x<=1);
    float pDiff=pf-p0;
    float bezier=x * x * x + float(3) * (x * x * (float(1) - x));
    return p0+bezier*pDiff;
}

float CubicBezier::CubicBezierFirstOrder(float p0, float pf, float x) {
    assert(x >= 0 && x <= 1);
    float pDiff = pf - p0;
    float bezier = float(6) * x * (float(1) - x);
    return bezier * pDiff;
}

float CubicBezier::CubicBezierSecondOrder(float p0, float pf, float x) {
    assert(x >= 0 && x <= 1);
    float pDiff = pf - p0;
    float bezier = float(6) - float(12) * x;
    return bezier * pDiff;
}

void DefaultSwingLegPlanner::Compute(float phase) {
    if(m_curveType==CurveType::cubicBezier){
        m_pInstant=m_cubicBezier.CubicBezierZeroOrder(m_pInitial,m_pFinal,phase);
        m_vInstant=m_cubicBezier.CubicBezierFirstOrder(m_pInitial,m_pFinal,phase)/m_swingPeriod;
        m_aInstant=m_cubicBezier.CubicBezierSecondOrder(m_pInitial,m_pFinal,phase)/(m_swingPeriod*m_swingPeriod);
        if(phase<0.5){
            m_pInstant[2]=m_cubicBezier.CubicBezierZeroOrder(m_pInitial[2],m_pInitial[2]+m_height,phase*2);
            m_vInstant[2]=m_cubicBezier.CubicBezierFirstOrder(m_pInitial[2],m_pInitial[2]+m_height,phase*2)*2/m_swingPeriod;
            m_aInstant[2]=m_cubicBezier.CubicBezierSecondOrder(m_pInitial[2],m_pInitial[2]+m_height,phase*2)*4/(m_swingPeriod*m_swingPeriod);
        }else{
            m_pInstant[2]=m_cubicBezier.CubicBezierZeroOrder(m_pInitial[2]+m_height,m_pFinal[2],phase*2-1);
            m_vInstant[2]=m_cubicBezier.CubicBezierFirstOrder(m_pInitial[2]+m_height,m_pFinal[2],phase*2-1)*2/m_swingPeriod;
            m_aInstant[2]=m_cubicBezier.CubicBezierSecondOrder(m_pInitial[2]+m_height,m_pFinal[2],phase*2-1)*4/(m_swingPeriod*m_swingPeriod);
        }

    }
}